/*
  Copyright (c) 2014 Dimitrij Gester

  "Smart RC TX" is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  "Smart RC TX" is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General
  Public License along with "Smart RC TX"; if not, write to the
  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
  Boston, MA  02111-1307  USA
*/

#ifndef MIXER_H
#define MIXER_H

#include <Arduino.h>
#include "Channels.h"
#include "MixerPersistor.h"

template<uint8_t slots>
class ChannelMixer
{
	protected:
	ChannelMixer(Channels<slots> &ch)
	: channels(ch)
	{
		memset(this->scale, 0, sizeof(this->scale));
		memset(this->value, 0, sizeof(this->value));
	}
	
	public:
	void setScale(uint8_t ch, int16_t sc)
	{
		this->scale[ch] = sc;
	}
	
	int16_t getScale(uint8_t ch)
	{
		return this->scale[ch];
	}

	void mix() {};

	void apply()
	{
		for(byte i = 0; i < slots; ++i)
		{
			this->channels[i] = this->value[i];
		}
	}
	
	uint16_t getPersistSize()
	{
		return sizeof(scale);
	}
	
	void serialize(MixerPersistor &p)
	{
		for(byte i = 0; i < slots; i++)
			p.putValue(this->scale[i]);
	}
	
	void deserialize(MixerPersistor &p)
	{
		for(byte i = 0; i < slots; i++)
		{
			if(!p.hasValue())
				return;
			
			this->scale[i] = p.getValue();
		}
	}
	protected:
	Channels<slots> &channels;
	int16_t scale[slots];
	int16_t value[slots];
};

/**
*	Trimmer. Scale ranges from -512 to 511
*/
template<uint8_t slots>
class TrimMixer : public ChannelMixer<slots>
{
	public:
	TrimMixer(Channels<slots> &ch)
	: ChannelMixer<slots>(ch)
	{
	}
	
	void mix()
	{
		for(byte i = 0; i < slots; ++i)
		{
			this->value[i] = constrain(this->channels[i] + this->scale[i], -512, 511);
		}
	}
};


/**
*	Scaler. Scale is the gain in percent to apply.
*/
template<uint8_t slots>
class ScalerMixer : public ChannelMixer<slots>
{
	public:
	ScalerMixer(Channels<slots> &ch)
	: ChannelMixer<slots>(ch)
	{
		for(byte i = 0; i < slots; ++i)
		{
			this->scale[i] = 100;
		}
	}
	
	void mix()
	{
		int32_t v;
		for(byte i = 0; i < slots; ++i)
		{
			v = (int32_t)(this->channels[i]);
			v *= this->scale[i];
			v /= 100;
			this->value[i] = constrain(v, -512, 511);
		}
	}
};

template<uint8_t slots>
class TwoWayMixer : public ChannelMixer<slots>
{
	protected:
	TwoWayMixer(Channels<slots> &ch)
	: ChannelMixer<slots>(ch)
	{
		memset(this->scale_N, 0, sizeof(this->scale_N));
	}
	
	public:
	void setScale(uint8_t ch, int16_t sc)
	{
		ChannelMixer<slots>::setScale(ch, sc);
	}

	void setScaleD(uint8_t ch, int16_t sc)
	{
		this->scale_N[ch] = sc;
	}

	int16_t getScaleD(uint8_t ch)
	{
		return this->scale_N[ch];
	}

	uint16_t getPersistSize()
	{
		return ChannelMixer<slots>::getPersistSize() + sizeof(scale_N);
	}
	
	void serialize(MixerPersistor &p)
	{
		ChannelMixer<slots>::serialize(p);

		for(byte i = 0; i < slots; i++)
		{
			p.putValue(this->scale_N[i]);
		}
	}
	
	void deserialize(MixerPersistor &p)
	{
		ChannelMixer<slots>::deserialize(p);
		
		for(byte i = 0; i < slots; i++)
		{
			if(!p.hasValue())
				return;
			
			this->scale_N[i] = p.getValue();
		}
	}
	
	protected:
	int16_t scale_N[slots];
};

/**
*	Limiter. Serves as a two-way scaler with separate gains for positive and negative input values. Each scale serves as a gain in percent.
*/
template<uint8_t slots>
class Limiter : public TwoWayMixer<slots>
{
	public:
	Limiter(Channels<slots> &ch)
	: TwoWayMixer<slots>(ch)
	{
		for(byte i = 0; i < slots; ++i)
		{
			this->scale[i] = 511;
			this->scale_N[i] = -512;
		}
	}

	void setScale(uint8_t ch, int16_t sc)
	{
		TwoWayMixer<slots>::setScale(ch, min((int16_t) abs((long)sc), 511));
	}

	void setScaleD(uint8_t ch, int16_t sc)
	{
		TwoWayMixer<slots>::setScaleD(ch, max((int16_t) -abs((long)sc), -512));
	}

	void mix()
	{
		for(byte i = 0; i < slots; ++i)
		{
			this->value[i] = constrain(this->channels[i], this->scale_N[i], this->scale[i]);
		}
	}
};

/**
*	Expo. Scale ranges from -100 to 100 as a weight for the amount of non-linearity.
*/
template<uint8_t slots>
class ExponentialMixer : public ChannelMixer<slots>
{
	public:
	ExponentialMixer(Channels<slots> &ch)
	: ChannelMixer<slots>(ch)
	{
	}
	
	void setScale(uint8_t ch, int16_t sc)
	{
		sc = constrain(sc, (int16_t)-100, (int16_t)100);
		if(sc < 0)
		sc /= 2;

		ChannelMixer<slots>::setScale(ch, sc);
	}

	void mix()
	{
		int32_t v;
		int32_t f;
		for(byte i = 0; i < slots; ++i)
		{
			if(this->scale[i] == 0)
			{
				this->value[i] = this->channels[i];
				continue;
			}
			
			// compute (v/span)^3 * span (with span = 512)
			// by scaling down the input value
			// with a factor as to stay within
			// the 32bit range and to
			// keep the final divisor a natural number.
			f = this->channels[i]/8;
			v = f * f * f;
			v /= (512);

			// scale exponential effect
			f = this->channels[i];
			v = v * (int32_t)this->scale[i]; 
			v += f * (100 - (int32_t)this->scale[i]);
			v /= 100;

			this->value[i] = constrain(v, (int16_t)-512, (int16_t)511);
		}
	}
};

/**
*	CrossMixer. Gain values are given (in percent) for each target channel separately, affecting it depending on the given source channel value.
*				Self-affecting gain values are ignored.
*/
template<uint8_t slots>
class CrossMixer : public TwoWayMixer<slots>
{
	public:
	CrossMixer(Channels<slots> &ch, uint8_t src)
	: TwoWayMixer<slots>(ch)
	, source(src)
	{
	}

	void setScale(uint8_t ch, int16_t sc)
	{
		if(ch == this->source)
		{
			sc = sc - 100;
		}
		
		TwoWayMixer<slots>::setScale(ch, sc);
	}

	void setScaleD(uint8_t ch, int16_t sc)
	{
		if(ch == this->source)
		{
			sc = sc - 100;
		}
		
		TwoWayMixer<slots>::setScaleD(ch, sc);
	}

	void mix()
	{
		int32_t v;
		for(byte i = 0; i < slots; ++i)
		{			
			v = this->channels[this->source];
			if(v >= 0)
				v *= this->scale[i];
			else
				v *= this->scale_N[i];
			
			v /= 100;
			
			this->value[i] = v;
		}
	}

	void apply()
	{
		for(byte i = 0; i < slots; ++i)
		{
			this->channels[i] =	this->channels[i] + this->value[i];
		}
	}
	
	private:
	uint8_t source;
};

#endif


